#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/pwm.h>
#include <zephyr/logging/log.h>
#include "include/relay.h"
#include "include/servo.h"
#include "include/pca9685.h"
#include <zephyr/drivers/i2c.h>
#include <math.h>

//#define DIR_PWM

LOG_MODULE_REGISTER(servo);
extern const struct device *const i2c_dev;
// extern const struct pwm_dt_spec *servo_head_x, *servo_head_y, *servo_fin_left_out,*servo_fin_left_in,
//                 *servo_fin_right_in,*servo_fin_right_out;

Servo_Sta Servo_Sta_g;
Servo_Ctr servo_init_angle = {
    .servo_head_x = 90,
    .servo_head_y = 90,
    .servo_fin_left_out = 90,
    .servo_fin_left_in = 90,
    .servo_fin_right_in = 90,
    .servo_fin_right_out = 90
};

void Servo_angle_init_cg(Servo_Ctr servo_init_angle_val) //往flash里面烧舵机初始化值
{
    //往flash写
    Servo_ctr_API(servo_init_angle_val);
}

int Servo_ctr_API(Servo_Ctr servo_val) 
{ 
    if(servo_val.servo_head_x > (servo_init_angle.servo_head_x + 45)){
        servo_val.servo_head_x = servo_init_angle.servo_head_x + 45;
    }
    if(servo_val.servo_head_x < (servo_init_angle.servo_head_x - 45)){
        servo_val.servo_head_x = servo_init_angle.servo_head_x - 45;
    }

    if(servo_val.servo_head_y < (servo_init_angle.servo_head_y - 45)){
        servo_val.servo_head_y = servo_init_angle.servo_head_y - 45;
    }
    if(servo_val.servo_head_y > (servo_init_angle.servo_head_y + 45)){
        servo_val.servo_head_y = servo_init_angle.servo_head_y + 45;
    }
    

    if(servo_val.servo_fin_left_out > (servo_init_angle.servo_fin_left_out + 90)){
        servo_val.servo_fin_left_out = servo_init_angle.servo_fin_left_out + 90;
    }
    if(servo_val.servo_fin_left_out < (servo_init_angle.servo_fin_left_out - 90)){
        servo_val.servo_fin_left_out = servo_init_angle.servo_fin_left_out - 90;
    }

    if(servo_val.servo_fin_left_in > (servo_init_angle.servo_fin_left_in + 180)){
        servo_val.servo_fin_left_in = servo_init_angle.servo_fin_left_in + 180;
    }
    if(servo_val.servo_fin_left_in < (servo_init_angle.servo_fin_left_in - 180)){
        servo_val.servo_fin_left_in = servo_init_angle.servo_fin_left_in - 180;
    }

    if(servo_val.servo_fin_right_in > (servo_init_angle.servo_fin_right_in + 180)){
        servo_val.servo_fin_right_in = servo_init_angle.servo_fin_right_in + 180;
    }
    if(servo_val.servo_fin_right_in < (servo_init_angle.servo_fin_right_in - 180)){
        servo_val.servo_fin_right_in = servo_init_angle.servo_fin_right_in - 180;
    }

    if(servo_val.servo_fin_right_out > (servo_init_angle.servo_fin_right_out + 90)){
        servo_val.servo_fin_right_out = servo_init_angle.servo_fin_right_out + 90;
    }
    if(servo_val.servo_fin_right_out < (servo_init_angle.servo_fin_right_out - 90)){
        servo_val.servo_fin_right_out = servo_init_angle.servo_fin_right_out - 90;
    }
    
    #ifdef DIR_PWM
    if (!device_is_ready(servo_head_x -> dev) || !device_is_ready(servo_head_y -> dev) || 
        !device_is_ready(servo_fin_left_out -> dev) || !device_is_ready(servo_fin_left_in -> dev) || 
        !device_is_ready(servo_fin_right_in -> dev) || !device_is_ready(servo_fin_right_out -> dev)) {
        /* Not ready, do not use */
        printk("servo: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        pwm_set_pulse_dt(servo_head_x, PWM_USEC(min_pulse + servo_val.servo_head_x * (max_pulse - min_pulse) / 180));
        pwm_set_pulse_dt(servo_head_y, PWM_USEC(min_pulse / 2 + servo_val.servo_head_y * (max_pulse - min_pulse) / 180));
        pwm_set_pulse_dt(servo_fin_left_out, PWM_USEC(min_pulse + servo_val.servo_fin_left_out * (max_pulse - min_pulse) / 180));
        pwm_set_pulse_dt(servo_fin_left_in, PWM_USEC(min_pulse + servo_val.servo_fin_left_in * (max_pulse - min_pulse) / 180));
        pwm_set_pulse_dt(servo_fin_right_in, PWM_USEC(min_pulse + servo_val.servo_fin_right_in * (max_pulse - min_pulse) / 180));
        pwm_set_pulse_dt(servo_fin_right_out, PWM_USEC(min_pulse + servo_val.servo_fin_right_out * (max_pulse - min_pulse) / 180));
        return 0;
    }
    #else
    if (!device_is_ready(i2c_dev)) {
        /* Not ready, do not use */
        LOG_ERR("I2C: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        setAngle(4,servo_val.servo_head_x);
        setAngle(5,servo_val.servo_head_y);
        setAngle(6,servo_val.servo_fin_left_out);
        setAngle(0,servo_val.servo_fin_left_in);
        setAngle(1,servo_val.servo_fin_right_in);
        setAngle(7,servo_val.servo_fin_right_out);
    return 0;
    }
    #endif

}


int Servo_ctr_single(uint8_t servo_num, float angle)  //舵机正常运转，不可调速，id为舵机编号，angle为可调角度
{
    switch(servo_num){
        case 4:
            if(angle > (servo_init_angle.servo_head_x + 45)){ //xy一样的初始值
                angle = servo_init_angle.servo_head_x + 45;
            }
            if(angle < (servo_init_angle.servo_head_x - 45)){
                angle = servo_init_angle.servo_head_x - 45;
            }
        break;

        case 5:
            if(angle > (servo_init_angle.servo_head_y + 45)){ //xy一样的初始值
                angle = servo_init_angle.servo_head_y + 45;
            }
            if(angle < (servo_init_angle.servo_head_y - 45)){
                angle = servo_init_angle.servo_head_y - 45;
            }
        break;

        case 6:
            if(angle > (servo_init_angle.servo_fin_left_out + 90)){
                angle = servo_init_angle.servo_fin_left_out + 90;
            }
            if(angle < (servo_init_angle.servo_fin_left_out - 90)){
                angle = servo_init_angle.servo_fin_left_out - 90;
            }
        break;
        
        case 0:
            if(angle > (servo_init_angle.servo_fin_left_in + 180)){
                angle = servo_init_angle.servo_fin_left_in + 180;
            }
            if(angle < (servo_init_angle.servo_fin_left_in - 180)){
                angle = servo_init_angle.servo_fin_left_in - 180;
            }
        break;

        case 1:
            if(angle > (servo_init_angle.servo_fin_right_in + 180)){
                angle = servo_init_angle.servo_fin_right_in + 180;
            }
            if(angle < (servo_init_angle.servo_fin_right_in - 180)){
                angle = servo_init_angle.servo_fin_right_in - 180;
            }
        break;

        case 7:
            if(angle > (servo_init_angle.servo_fin_right_out + 90)){
                angle = servo_init_angle.servo_fin_right_out + 90;
            }
            if(angle < (servo_init_angle.servo_fin_right_out - 90)){
                angle = servo_init_angle.servo_fin_right_out - 90;
            }
        break;
        
    }      

    #ifdef DIR_PWM
     if (!device_is_ready(servo_head_x -> dev) || !device_is_ready(servo_head_y -> dev) || !device_is_ready(servo_fin_left_out -> dev) || !device_is_ready(servo_fin_left_in -> dev) || !device_is_ready(servo_fin_right_in -> dev) || !device_is_ready(servo_fin_right_out -> dev)) {
        /* Not ready, do not use */
        printk("servo: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        pwm_set_pulse_dt(find_pwmdt_by_num(servo_num), PWM_USEC(min_pulse + angle * (max_pulse - min_pulse) / 180));
        return 0;    
    }
    #else
    if (!device_is_ready(i2c_dev)) {
        /* Not ready, do not use */
        LOG_ERR("I2C: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        setAngle(servo_num,angle);
        return 0;
    }
    #endif

 }

// static int Servo_init_angle(Servo_Ctr servo_val){
//     float angle;
//     uint32_t off = 0;
//     int ret;
//     #ifdef DIR_PWM
//     ret = Servo_ctr_API(servo_val);
//     #else
//     angle = servo_val.servo_head_x;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(0,0,off);
//     angle = servo_val.servo_head_y;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(1,0,off);
//     angle = servo_val.servo_fin_left_out;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(2,0,off);
//     angle = servo_val.servo_fin_left_in;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(3,0,off);
//     angle = servo_val.servo_fin_right_in;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(4,0,off);
//     angle = servo_val.servo_fin_right_out;
//     off = (uint32_t)(145+angle*2.4);
// 	ret |= PCA9685_setPWM(5,0,off);
//     #endif
//     return ret;
// }

int Servo_init(void)
{
    #ifdef DIR_PWM
     if (!device_is_ready(servo_head_x -> dev) || !device_is_ready(servo_head_y -> dev) || !device_is_ready(servo_fin_left_out -> dev) || !device_is_ready(servo_fin_left_in -> dev) || !device_is_ready(servo_fin_right_in -> dev) || !device_is_ready(servo_fin_right_out -> dev)) {
        /* Not ready, do not use */
        printk("servo: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        //Servo_init_angle();
        return 0;
    }
    #else
    if (!device_is_ready(i2c_dev)) {
        /* Not ready, do not use */
        LOG_ERR("I2C: Device is not ready.\n");
        return -ENODEV;
	}
    else{
        PCA9685_Init(50); //设置频率
        //Servo_init_angle();
        return 0;
    }
    #endif
}


// const struct pwm_dt_spec* find_pwmdt_by_num(uint8_t servo_num){
//     switch(servo_num){
//         case 0:
//             return servo_head_x;
//         case 1:
//             return servo_head_y;
//         case 2:
//             return servo_fin_left_out;
//         case 3:
//             return servo_fin_left_in;
//         case 4:
//             return servo_fin_right_in;
//         case 5:
//             return servo_fin_right_out;
//         default:
//             return servo_head_x;
//     }
// }